%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画出环境地图及路径%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function plotMap_Path(map,C)
% map 地图矩阵
global Path;
n = size(map);
step = 1;
a = 0 : step :n(1);
b = 0 : step :n(2);
figure(length(Path)+1)
axis([0 n(2) 0 n(1)]); %设置地图横纵尺寸
set(gca,'xtick',b,'ytick',a,'GridLineStyle','-',...
'xGrid','on','yGrid','on');
hold on
r = 1;
for i=1:n(1)         %设置障碍物的左下角点的x,y坐标
    for j=1:n(2)
        if map(i,j)==1
            p(r,1)=j-1;
            p(r,2)=i-1;
            fill([p(r,1) p(r,1) + step p(r,1) + step p(r,1)],...
                 [p(r,2) p(r,2) p(r,2) + step p(r,2) + step ],'k');
            r=r+1;
            hold on
        end
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%栅格数字标识%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x_text = 1:1:n(1)*n(2); %产生所需数值.
for i = 1:1:n(1)*n(2)
    [row,col] = ind2sub([n(2),n(1)],i);
    text(row-0.9,col-0.5,num2str(x_text(i)),'FontSize',8,'Color','0.7 0.7 0.7');
end
hold on
axis square
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%得到路径坐标%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 n = size(map);
 z = size(Path,1);               %路径条数
 for r=1:z
     M = size(Path{r},2);         %每条路径的路径点数    
     for m=1:M                  
         if rem(Path{r}(m),n(2))==0
             X(r,m)=n(2);
             Y(r,m)= floor(Path{r}(m)/n(2));
         else
             X(r,m)= rem(Path{r}(m),n(2));
             Y(r,m)= floor(Path{r}(m)/n(2))+1;
         end
     end
 end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画出路径%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 for i=1:length(Path)
     for j=2:size(Path{i},2)
          place = 0.5-0.01*i;
          plot([X(i,j-1)-place,X(i,j)-place],[Y(i,j-1)-place,Y(i,j)-place],'Color',C(i,:),'LineWidth',2);%画出路径
%           plot([X(i,j-1)-place,X(i,j)-place],[Y(i,j-1)-place,Y(i,j)-place],'Color',C(1,:),'LineWidth',2);
          hold on
     end
end



